﻿package com.googlecode.gingerman.robot;

import com.googlecode.gingerman.world.geometry.Circle;
import com.googlecode.gingerman.world.geometry.Coordinates;
import com.googlecode.gingerman.world.geometry.Vector;

public class Chassis implements AIChassis {
	private double speed, nextSpeed, angle, nextAngle, radius, x, y;
	private Robot robot;
	private Circle circle;
	private Coordinates coordinates;

	public Chassis(double x, double y, double angle, double radius, Robot robot) {
		this.robot = robot;
		this.x = x;
		this.y = y;
		this.angle=angle;
		this.radius = radius;
		speed=0;
		nextSpeed=0;
		nextAngle=0;
		coordinates = new Coordinates(this.x, this.y);
		circle = new Circle(coordinates, this.radius);
	}
	
	public double getX() {
		return x;
	}

	public double getY() {
		return y;
	}
	
	public double getSpeed() {
		return speed;
	}

	public double getAngle() {
		return this.angle;
	}

	public double getRadius() {
		return this.radius;
	}
	
	public Circle getCircle() {
		return this.circle;
	}

	public void setNextSpeed(double speed) {
		this.nextSpeed = speed;
	}

	public void setNextAngle(double angle) {
		this.nextAngle = angle;
	}
	
	public void step() {
		if (nextSpeed>10) nextSpeed=10;
		speed=nextSpeed; angle=nextAngle;
		coordinates = new Coordinates(x, y);
		circle = new Circle(coordinates, radius);
		Vector vector = new Vector(coordinates);
		Vector tmp = new Vector(nextAngle, nextSpeed);
		vector = vector.add(tmp);
		
		boolean f1 = false;
		coordinates = new Coordinates(vector.getX(), vector.getY());
		circle = new Circle(coordinates, radius);
		
		double oldX=x, oldY=y;
		
		if ((vector.getX()+radius)>robot.getWorld().getWidth()) x=robot.getWorld().getWidth()-radius;
		else if ((vector.getX()-radius)<0) x=radius;
		else x = vector.getX();
		
		if ((vector.getY()+radius)>robot.getWorld().getHeight()) y=robot.getWorld().getHeight()-radius;
		else if ((vector.getY()-radius)<0) y=radius;
		else y = vector.getY(); 

		for (int i = 0; i < robot.getWorld().getRobots().size(); i++) {
			if (robot!=robot.getWorld().getRobots().get(i)){
				f1 = circle.intersectFigure(robot.getWorld().getRobots().get(i).getCircle());
				if (f1) {
					coordinates=new Coordinates(x,y);
					tmp=new Vector(coordinates);
					coordinates=new Coordinates(robot.getWorld().getRobots().get(i).getX(), robot.getWorld().getRobots().get(i).getY());
					Vector tmp2=new Vector(coordinates);
					double lengthtmp=tmp.getLength(),lengthtmp2=tmp2.getLength();
					if (lengthtmp<=lengthtmp2)
						tmp2=tmp2.substract(tmp);
					else
						tmp2=tmp.substract(tmp2);
					Vector tmp3=new Vector(tmp2.getAngle(), robot.getRadius()+robot.getWorld().getRobots().get(i).getRadius());
					tmp3=tmp3.substract(tmp2);
					if (lengthtmp<=lengthtmp2)
						tmp=tmp.substract(tmp3);
					else
						tmp=tmp.add(tmp3);
					x=tmp.getX();
					y=tmp.getY();
				}
			}
		}
		
		for (int i = 0; i < robot.getWorld().getWalls().size(); i++) {
			f1 = circle.intersectFigure(robot.getWorld().getWalls().get(i).getRectangle());
			if (f1) {
				coordinates=new Coordinates(oldX,oldY);
				tmp=new Vector(coordinates);
				
				double massivOfLength[]=new double[4];
				Vector massivOfVector[]=new Vector[4];
				
				for (int j=0; j<=3; j++){
					massivOfVector[j]=robot.getWorld().getWalls().get(i).getRectangle().getVertices().get(j);
					massivOfLength[j]=massivOfVector[j].substract(tmp).getLength();
				}
				
				double Lchg;
				double Achg;
				Vector Vchg;
				
				for (int k=0; k<=3; k++){
					for (int j=0; j<=2; j++){
						if (massivOfLength[j]>massivOfLength[j+1]){
							Lchg=massivOfLength[j];
							massivOfLength[j]=massivOfLength[j+1];
							massivOfLength[j+1]=Lchg;
							Vchg=massivOfVector[j];
							massivOfVector[j]=massivOfVector[j+1];
							massivOfVector[j+1]=Vchg;
						}
					}
				}
				
				coordinates=new Coordinates(x,y);
				Vector tmp2=new Vector(coordinates);
				
				double angle0=Math.abs(massivOfVector[0].substract(tmp).getAngle());
				if (angle0>Math.PI/2) angle0=Math.abs(angle0-Math.PI);
				double angle1=Math.abs(massivOfVector[1].substract(tmp).getAngle());
				if (angle1>Math.PI/2) angle1=Math.abs(angle1-Math.PI);
				tmp=tmp2.substract(tmp);
				
				if (Math.abs(Math.abs(tmp.getAngle())-angle0)<Math.abs(Math.abs(tmp.getAngle())-angle1)){
					tmp=tmp2.substract(massivOfVector[0]);
					tmp2=massivOfVector[1].substract(massivOfVector[0]);
					Achg=Math.abs(tmp.getAngle()-tmp2.getAngle());
					if (Achg>Math.PI/2) Achg=Math.abs(Achg-Math.PI);
					Lchg=circle.getRadius()*Math.sin(Achg);
					tmp=tmp.normalize();
					tmp=tmp.multiplyByScalar(Lchg);
					tmp=massivOfVector[0].add(tmp);
					x=tmp.getX();
					y=tmp.getY();
				}else{
					tmp=tmp2.substract(massivOfVector[1]);
					tmp2=massivOfVector[0].substract(massivOfVector[1]);
					Achg=Math.abs(tmp.getAngle()-tmp2.getAngle());
					if (Achg>Math.PI/2) Achg=Math.abs(Achg-Math.PI);
					Lchg=circle.getRadius()*Math.sin(Achg);
					tmp=tmp.normalize();
					tmp=tmp.multiplyByScalar(Lchg);
					tmp=massivOfVector[1].add(tmp);
					x=tmp.getX();
					y=tmp.getY();
				}

			}
		}
		
		coordinates = new Coordinates(x, y);
		circle = new Circle(coordinates, radius);
		
	}
	
}